#include <MPU6050_tockn.h>
#include "MyBLE.h"

MPU6050 mpu6050(Wire);
MyBLE myBLE;

float balanceX=0, balanceY=0;
float kp_x = 4, ki_x =0.05, kd_x =0.5;
float kp_y = 4, ki_y =0.05, kd_y =0.5;

float xpwm,ypwm,xpwm_a,xpwm_b,ypwm_l,ypwm_r;
float angleX,angleY,gyroX,gyroY;
float biasX,biasY;
float integrateX,integrateY;

void pwm_do(){
  mpu6050.update();

  angleX = mpu6050.getAngleX();
  angleY = mpu6050.getAngleY();
  gyroX = mpu6050.getGyroX();
  gyroY = mpu6050.getGyroY();

  biasX = angleX - balanceX;
  biasY = angleY - balanceY;

  integrateX += biasX;
  integrateY += biasY;

  integrateX=constrain(integrateX, -500, 500);
  integrateY=constrain(integrateY, -500, 500);

  int xpwm_pid=kp_x * biasX + ki_x * integrateX + kd_x * gyroX;
  int ypwm_pid=kp_y * biasY + ki_y * integrateY + kd_y * gyroY;

  xpwm_pid = xpwm_pid / 2;
  ypwm_pid = ypwm_pid / 2;

  xpwm_pid=constrain(xpwm_pid, -100, 100);
  ypwm_pid=constrain(ypwm_pid, -100, 100);

  xpwm_a = xpwm - xpwm_pid;
  xpwm_b = xpwm + xpwm_pid;
  ypwm_l = ypwm - ypwm_pid;
  ypwm_r = ypwm + ypwm_pid;

  xpwm_a = constrain(xpwm_a, 0, 250);
  xpwm_b = constrain(xpwm_b, 0, 250);
  ypwm_l = constrain(ypwm_l, 0, 250);
  ypwm_r = constrain(ypwm_r, 0, 250);
  
}

void onBLEReceived(std::string &s){
    char val=Udp.read();
    Serial.println(val);
    switch (val){
      case '0':
      kp_x -= 1;
      break;
      case '1':
      kp_x += 1;
      break;
      case '2':
      ki_x -= 0.05;
      break;
      case '3':
      ki_x += 0.05;
      break;
      case '4':
      kd_x -= 0.1;
      break;
      case '5':
      kd_x += 0.1;
      break;
    }
    kp_y = kp_x;
    ki_y = ki_x;
    kd_y = kd_x;

    Serial.println("data:"+String(val));
  }
  //myBLE.sendBackData(s);
}

void setup() {
  Serial.begin(9600);

  myBLE=MyBLE();
  myBLE.initBLE("BLE_OF_ESP32","b408e1a0-3d8a-11ed-b878-0242ac120002","de045162-3d97-11ed-b878-0242ac120002",&onBLEReceived);

  Wire.begin(2,21);
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  
  pinMode(20,OUTPUT);
  digitalWrite(20, 0);
}

void loop() {
  // delay(2000);
  // mpu6050.update();
  // angleX = mpu6050.getAngleX();
  // std::string dd=std::to_string(angleX);
  // myBLE.sendBackData(dd);
  //pwm_do();
  //motor_do();
}
